#include <pose_solver/pose_solver.hpp>
using namespace std::chrono_literals;

namespace wmj
{
    PoseSolver::PoseSolver(std::string name) : Node(name)
    {
        // 不同模型装甲板数量，STANDARD为4个装甲板，BALANCE为2个装甲板，OUTPOST为3个装甲板
        model_armors[0] = 4;
        model_armors[1] = 2;
        model_armors[2] = 3;

        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
        status_ptr_ = nullptr;
        enable_shoot = true;

        this->declare_parameter<bool>("debug", debug);
        this->declare_parameter<float>("timeout", m_timeout);
        this->declare_parameter<double>("shoot_interval", shoot_interval);
        this->declare_parameter<std::string>("Gimbal.shooter_link", shooter_link_);
        this->declare_parameter<std::string>("Gimbal.shooter_pitch_link", shooter_pitch_link);
        this->declare_parameter<std::string>("Gimbal.shooter_yaw_link", shooter_yaw_link);
        this->declare_parameter<bool>("Gimbal.fix_on", fix_on_);
        this->declare_parameter<bool>("Gimbal.track_center", m_track_center);
        this->declare_parameter<float>("Gimbal.pitch_off", pitch_off_);
        this->declare_parameter<float>("Gimbal.yaw_off", yaw_off_);
        this->declare_parameter<bool>("BigYaw.enable_control", m_big_yaw_control);
        this->declare_parameter<bool>("BigYaw.Filter.enable", m_enable_big_yaw_filter);
        this->declare_parameter<float>("BigYaw.Filter.alpha", m_filter_alpha);
        this->declare_parameter<float>("Aimer.switch_threshold", m_switch_threshold);
        this->declare_parameter<float>("Aimer.time_off", m_time_off);
        this->declare_parameter<float>("Aimer.force_aim_palstance_threshold", m_force_aim_palstance_threshold);
        this->declare_parameter<float>("Aimer.aim_angle_tolerance", m_aim_angle_tolerance);
        this->declare_parameter<float>("Aimer.aim_pose_tolerance", m_aim_pose_tolerance);
        this->declare_parameter<float>("Aimer.CenterTrack.aim_center_palstance_threshold", m_aim_center_palstance_threshold);
        this->declare_parameter<float>("Aimer.CenterTrack.m_switch_trackmode_threshold", m_switch_trackmode_threshold);
        this->declare_parameter<float>("Aimer.CenterTrack.aim_center_angle_tolerance", m_aim_center_angle_tolerance);
        this->declare_parameter<int>("Aimer.all_white_tolerance_stop_shoot", all_white_tolerance_stop_shoot_);
        this->declare_parameter<double>("Bullet.bullet_speed", m_bullet_speed);

        this->get_parameter<bool>("debug", debug);
        this->get_parameter<float>("timeout", m_timeout);
        this->get_parameter<double>("shoot_interval", shoot_interval);
        this->get_parameter<std::string>("Gimbal.shooter_link", shooter_link_);
        this->get_parameter<std::string>("Gimbal.shooter_pitch_link", shooter_pitch_link);
        this->get_parameter<std::string>("Gimbal.shooter_yaw_link", shooter_yaw_link);
        this->get_parameter<bool>("Gimbal.fix_on", fix_on_);
        this->get_parameter<bool>("Gimbal.track_center", m_track_center);
        this->get_parameter<float>("Gimbal.pitch_off", pitch_off_);
        this->get_parameter<float>("Gimbal.yaw_off", yaw_off_);
        this->get_parameter<bool>("BigYaw.enable_control", m_big_yaw_control);
        this->get_parameter<bool>("BigYaw.Filter.enable", m_enable_big_yaw_filter);
        this->get_parameter<float>("BigYaw.Filter.alpha", m_filter_alpha);
        this->get_parameter<float>("Aimer.switch_threshold", m_switch_threshold);
        this->get_parameter<float>("Aimer.time_off", m_time_off);
        this->get_parameter<float>("Aimer.force_aim_palstance_threshold", m_force_aim_palstance_threshold);
        this->get_parameter<float>("Aimer.aim_angle_tolerance", m_aim_angle_tolerance);
        this->get_parameter<float>("Aimer.aim_pose_tolerance", m_aim_pose_tolerance);
        this->get_parameter<float>("Aimer.CenterTrack.aim_center_palstance_threshold", m_aim_center_palstance_threshold);
        this->get_parameter<float>("Aimer.CenterTrack.m_switch_trackmode_threshold", m_switch_trackmode_threshold);
        this->get_parameter<float>("Aimer.CenterTrack.aim_center_angle_tolerance", m_aim_center_angle_tolerance);
        this->get_parameter<int>("Aimer.all_white_tolerance_stop_shoot", all_white_tolerance_stop_shoot_);
        this->get_parameter<double>("Bullet.bullet_speed", m_bullet_speed);

        /************* MPC **************/
        this->declare_parameter<int>("MPC.step", m_step);
        this->declare_parameter<double>("MPC.dt", m_dt);
        this->declare_parameter<bool>("use_mpc", m_use_mpc);
        this->declare_parameter<double>("MPC.process_error_weight.Q0", m_mpc_param.process_error_weight[0]);
        this->declare_parameter<double>("MPC.process_error_weight.Q1", m_mpc_param.process_error_weight[1]);
        this->declare_parameter<double>("MPC.process_error_weight.Q2", m_mpc_param.process_error_weight[2]);
        this->declare_parameter<double>("MPC.process_error_weight.Q3", m_mpc_param.process_error_weight[3]);

        this->declare_parameter<double>("MPC.target_error_weight.F0", m_mpc_param.target_error_weight[0]);
        this->declare_parameter<double>("MPC.target_error_weight.F1", m_mpc_param.target_error_weight[1]);
        this->declare_parameter<double>("MPC.target_error_weight.F2", m_mpc_param.target_error_weight[2]);
        this->declare_parameter<double>("MPC.target_error_weight.F3", m_mpc_param.target_error_weight[3]);

        this->declare_parameter<double>("MPC.process_control_weight.R0", m_mpc_param.process_control_weight[0]);
        this->declare_parameter<double>("MPC.process_control_weight.R1", m_mpc_param.process_control_weight[1]);

        this->get_parameter<int>("MPC.step", m_step);
        this->get_parameter<double>("MPC.dt", m_dt);
        this->get_parameter<bool>("use_mpc", m_use_mpc);
        this->get_parameter<double>("MPC.process_error_weight.Q0", m_mpc_param.process_error_weight[0]);
        this->get_parameter<double>("MPC.process_error_weight.Q1", m_mpc_param.process_error_weight[1]);
        this->get_parameter<double>("MPC.process_error_weight.Q2", m_mpc_param.process_error_weight[2]);
        this->get_parameter<double>("MPC.process_error_weight.Q3", m_mpc_param.process_error_weight[3]);

        this->get_parameter<double>("MPC.target_error_weight.F0", m_mpc_param.target_error_weight[0]);
        this->get_parameter<double>("MPC.target_error_weight.F1", m_mpc_param.target_error_weight[1]);
        this->get_parameter<double>("MPC.target_error_weight.F2", m_mpc_param.target_error_weight[2]);
        this->get_parameter<double>("MPC.target_error_weight.F3", m_mpc_param.target_error_weight[3]);

        this->get_parameter<double>("MPC.process_control_weight.R0", m_mpc_param.process_control_weight[0]);
        this->get_parameter<double>("MPC.process_control_weight.R1", m_mpc_param.process_control_weight[1]);
        /************* END MPC **************/

        top_sub_ = this->create_subscription<base_interfaces::msg::KinematicStatus>("/Top_status", 1, std::bind(&PoseSolver::top_cb_, this, std::placeholders::_1));

        Gimbal_pose_pub_ = this->create_publisher<base_interfaces::msg::GimbalPose>(
            "setGimbalAngle", rclcpp::QoS(rclcpp::KeepLast(1)));
        Big_yaw_pose_pub_ = this->create_publisher<base_interfaces::msg::BigYaw>(
            "/setBigYaw", rclcpp::QoS(rclcpp::KeepLast(1)));
        shoot_pub_ = this->create_publisher<base_interfaces::msg::Shooter>(
            "shootSome", rclcpp::QoS(rclcpp::KeepLast(1)));
        debug_pub = this->create_publisher<base_interfaces::msg::DebugOfKinematic>("debug", 2);
        bt_sub = this->create_subscription<base_interfaces::msg::BtAimer>("/BtAimer", 1, std::bind(&PoseSolver::bt_cb, this, std::placeholders::_1));
        Big_yaw_pose_sub_ = this->create_subscription<base_interfaces::msg::BigYaw>("/getBigYaw", 1, std::bind(&PoseSolver::yaw_cb, this, std::placeholders::_1));

        bullet_speed_sub = this->create_subscription<std_msgs::msg::Float32>("getBulletSpeed", 1, std::bind(&PoseSolver::bullet_speed_cb, this, std::placeholders::_1));

        mpc_vel_pub_ = this->create_publisher<base_interfaces::msg::GimbalPose>(
            "setGimbalSpeed", rclcpp::QoS(rclcpp::KeepLast(1)));

        // timer_ptr_ = this->create_wall_timer(5ms, std::bind(&PoseSolver::control_callback, this));    //200帧控制帧率

        if (m_use_mpc)
        {
            setParam(m_mpc_param); // 初始化 mpc 的矩阵
            setMpcMatrix();
        }

        timer_ptr_ = this->create_wall_timer(5ms, std::bind(&PoseSolver::control_callback, this)); // 200帧控制帧率

        if (debug) // 调试模式才发布预测装甲板
        {
            armors_pub = this->create_publisher<base_interfaces::msg::Armors>("test", 1);
        }
    }

    void PoseSolver::yaw_cb(const base_interfaces::msg::BigYaw &msg)
    {
        m_big_yaw = msg.angle;
    }

    void PoseSolver::bullet_speed_cb(const std_msgs::msg::Float32 &msg)
    {
        while (bullet_speed_list.size() >= slide_window_num)
        {
            bullet_speed_list.erase(bullet_speed_list.begin());
        }
        bullet_speed_list.push_back(msg.data);
        std::cout << " bullet_speed111: " << m_bullet_speed << std::endl;
        if (bullet_speed_list.size() < slide_window_num)
            return;
        float bullet_speed = 0;
        for (auto bs : bullet_speed_list)
        {
            bullet_speed += (bs / slide_window_num);
        }
        m_bullet_speed = bullet_speed;
        return;
    }

    void PoseSolver::setParam(mpcParam param)
    {

        for (int i = 0; i < _D_STATE_; i++)
        {
            m_process_error_weight[i] = param.process_error_weight[i];
            m_target_error_weight[i] = param.target_error_weight[i];
        }

        for (int i = 0; i < _D_CTRL_; i++)
        {
            m_process_control_weight[i] = param.process_control_weight[i];
        }
    }

    void PoseSolver::control_callback()
    {
        mtx.lock();
        if (!keep_scan)
        {
            m_filter_init = false;
            mtx.unlock(); // 每个return都要解锁，不然model永远不更新
            base_interfaces::msg::Shooter shoot_msg;
            shoot_msg.bulletnum = 0;
            shoot_pub_->publish(shoot_msg);
            base_interfaces::msg::BigYaw BY_msg;
            BY_msg.mode = wmj::BigYawAngle | wmj::BigYawVisionControl;
            BY_msg.angle = m_big_yaw;
            Big_yaw_pose_pub_->publish(BY_msg);
            // base_interfaces::msg::GimbalPose gimbal_msg;
            // base_interfaces::msg::GimbalPose target_gimbal_pose;
            // target_gimbal_pose.pitch = 0;
            // target_gimbal_pose.yaw = 0;
            // msg.message_owner = GIMBAL_CONTROL_PERMISSION::SCAN;
            // Gimbal_pose_pub_->publish(target_gimbal_pose)
            return;
        }
        else if (status_ptr_ == nullptr)
        {
            m_filter_init = false;
            mtx.unlock(); // 每个return都要解锁，不然model永远不更新
            std::cout << "null model" << std::endl;
            base_interfaces::msg::Shooter shoot_msg;
            shoot_msg.bulletnum = 0;
            shoot_pub_->publish(shoot_msg);
            return;
        }
        else if (status_ptr_->is_track) // 是否是自瞄状态
        {
            builtin_interfaces::msg::Time time = this->now();
            builtin_interfaces::msg::Time status_time = status_ptr_->header.stamp;

            double time_now = time.sec + time.nanosec * 1e-9;
            double time_status = status_time.sec + status_time.nanosec * 1e-9;
            if (time_now - time_status > m_timeout)
            {
                status_ptr_ = nullptr;
                std::cout << "TIMEOUT!" << std::endl;
                mtx.unlock(); // 每个return都要解锁，不然model永远不更新
                return;
            }
            geometry_msgs::msg::Pose target_armor_pose; // 获取目标装甲板位姿
            if (status_ptr_->enable)                    // model模型是否存在
            {
                // 判断是否锁中心
                if (m_track_center && fabs(status_ptr_->palstance) > m_switch_trackmode_threshold + m_aim_center_palstance_threshold)
                {
                    m_enable_center_track = true;
                }
                else if (m_track_center && fabs(status_ptr_->palstance) < m_aim_center_palstance_threshold)
                {
                    m_enable_center_track = false;
                }
                else if (!m_track_center)
                    m_enable_center_track = false;

                // 计算预测时间，即子弹飞行时间 + 建模时间(model_time_off) + time_off
                double start_time, end_time, model_time_off;
                start_time = status_ptr_->header.stamp.sec + status_ptr_->header.stamp.nanosec * 1e-9;
                builtin_interfaces::msg::Time t = this->now();
                end_time = t.sec + t.nanosec * 1e-9;
                model_time_off = end_time - start_time;
                if (debug)
                {
                    std::cout << "model_time_off: " << model_time_off << std::endl;
                }
                // 获取模型当前时刻要击打的装甲板位姿
                if (!m_enable_center_track) // 不锁中心
                {
                    if (!getClosestArmorPose(model_time_off, target_armor_pose))
                    {
                        mtx.unlock();
                        return;
                    }
                }
                else // 锁中心
                {
                    if (!getFacingArmorPose(model_time_off, target_armor_pose))
                    {
                        mtx.unlock();
                        return;
                    }
                }

                if (debug)
                {
                    this->get_parameter<float>("Aimer.time_off", m_time_off);
                    this->get_parameter<float>("Aimer.time_off", m_time_off_outpost);
                }
                double predict_time = getDistance(cv::Point3d(target_armor_pose.position.x, target_armor_pose.position.y, target_armor_pose.position.z)) / m_bullet_speed + model_time_off + m_time_off;
                if (status_ptr_->model_type == 2)
                {
                    std::cout << "bullet_speed" << m_bullet_speed << std::endl;
                    predict_time = getDistance(cv::Point3d(target_armor_pose.position.x, target_armor_pose.position.y, target_armor_pose.position.z)) / m_bullet_speed + model_time_off + m_time_off;
                }

                if (debug)
                {
                    std::cout << "predict_time: " << predict_time << std::endl;
                }

                // 获取预测时间后的装甲板位姿
                if (!m_enable_center_track)
                {
                    if (!getClosestArmorPose(predict_time, target_armor_pose))
                    {
                        mtx.unlock();
                        return;
                    }
                }
                else
                {
                    if (!getFacingArmorPose(predict_time, target_armor_pose))
                    {
                        mtx.unlock();
                        return;
                    }
                }

                // 控制大Yaw
                if (m_big_yaw_control)
                {
                    double roll, pitch, big_yaw;
                    geometry_msgs::msg::TransformStamped t;
                    auto now = this->now();
                    bool sign = true;
                    // 获取云台yaw角
                    try
                    {
                        t = tf_buffer_->lookupTransform(
                            base_link_, "body",
                            tf2::TimePoint());
                    }
                    catch (const tf2::TransformException &ex)
                    {
                        // RCLCPP_INFO(
                        //     this->get_logger(), "Could not transform %s to %s: %s",
                        //     tar_frame.c_str(), src_frame.c_str(), ex.what());
                        sign = false;
                    }
                    if (!sign)
                    // if (!getTransform(base_link_, "body", now, t))

                    {
                        std::cout << "false 3!" << std::endl;
                    }
                    else
                    {
                        tf2::Quaternion tf_q;
                        tf2::fromMsg(t.transform.rotation, tf_q);
                        tf2::Matrix3x3(tf_q).getRPY(roll, pitch, big_yaw);
                        if (m_enable_big_yaw_filter && m_filter_init == false)
                        {
                            m_filter_last_output = _std_radian(atan2(status_ptr_->center.y, status_ptr_->center.x));
                            m_filter_init = true;
                        }
                        float angle_output = _std_radian(atan2(status_ptr_->center.y, status_ptr_->center.x));
                        std::cout << "BY_angle" << angle_output << std::endl;
                        base_interfaces::msg::BigYaw BY_msg;
                        BY_msg.mode = wmj::BigYawAngle | wmj::BigYawVisionControl;
                        if (m_enable_big_yaw_filter)
                        {
                            BY_msg.angle = m_filter_last_output * (1 - m_filter_alpha) + angle_output * m_filter_alpha;
                            m_filter_last_output = BY_msg.angle;
                        }
                        else
                            BY_msg.angle = angle_output;
                        if (debug)
                        {
                            std::cout << "big yaw angle: " << BY_msg.angle << std::endl;
                        }
                        // if(fabs(_std_radian(BY_msg.angle - big_yaw)) > fabs(_std_radian(big_yaw + PI - BY_msg.angle)))
                        //     BY_msg.angle = _std_radian(BY_msg.angle + PI);
                        // if(isnan(BY_msg.angle))
                        // {
                        //     std::cout<<"BY nan ----- reset!!!"<<std::endl;
                        //     return;
                        // }
                        std::cout << "BY_msg.angle: " << BY_msg.angle << std::endl;
                        std::cout << "BYcur_pose.yaw " << big_yaw << std::endl;
                        Big_yaw_pose_pub_->publish(BY_msg);
                    }
                    // double big_yaw;
                    // big_yaw = m_big_yaw;
                    // if(m_enable_big_yaw_filter && m_filter_init == false)
                    //     {
                    //         m_filter_last_output = _std_radian(atan2(status_ptr_->center.y, status_ptr_->center.x));
                    //         m_filter_init = true;
                    //     }
                    // float angle_output = _std_radian(atan2(status_ptr_->center.y, status_ptr_->center.x));
                    // base_interfaces::msg::BigYaw BY_msg;
                    // BY_msg.mode = wmj::BigYawAngle | wmj::BigYawVisionControl;
                    // if(m_enable_big_yaw_filter)
                    // {
                    //     BY_msg.angle = m_filter_last_output * (1 - m_filter_alpha) + angle_output * m_filter_alpha;
                    //     m_filter_last_output = BY_msg.angle;
                    // }
                    // else
                    //     BY_msg.angle = angle_output;
                    // if (debug)
                    // {
                    //     std::cout << "big yaw angle: " << BY_msg.angle << std::endl;
                    // }
                    // if(fabs(_std_radian(BY_msg.angle - big_yaw)) > fabs(_std_radian(big_yaw + PI - BY_msg.angle)))
                    //     BY_msg.angle = _std_radian(BY_msg.angle + PI);
                    // std::cout<<"BY_msg.angle: "<<BY_msg.angle<<std::endl;
                    // std::cout<<"BYcur_pose.yaw "<<big_yaw<<std::endl;
                    // Big_yaw_pose_pub_->publish(BY_msg);
                }
            }
            else // 没有模型，直接击打检测到的装甲板
            {
                if (status_ptr_->armors.size() > 0)
                {
                    target_armor_pose = status_ptr_->armors[0].armor_pose;
                    geometry_msgs::msg::TransformStamped t;
                    if (getTransform(shooter_link_, base_link_, status_ptr_->header.stamp, t)) // 获取绝对系到电机系的坐标变化
                    {
                        tf2::doTransform(target_armor_pose, target_armor_pose, t); // 将装甲板位姿从绝对系转到电机系
                    }
                    else
                    {
                        mtx.unlock();
                        return;
                    }
                }
                else
                {
                    mtx.unlock();
                    std::cout << "not have armor to aim" << std::endl;
                    return;
                }
            }
            base_interfaces::msg::GimbalPose target_gimbal_pose;
            if (!getAngle(target_armor_pose, m_bullet_speed, target_gimbal_pose)) // 获取目标云台位姿
            {
                mtx.unlock();
                return;
            }
            target_gimbal_pose.message_owner = wmj::GIMBAL_CONTROL_PERMISSION::AIMER;
            if (!m_use_mpc)
            {
                Gimbal_pose_pub_->publish(target_gimbal_pose);
            }
            else
            {
                base_interfaces::msg::GimbalPose target_gimbal_speed;
                if (!getGimbalSpeed(target_gimbal_speed))
                {
                    mtx.unlock();
                    return;
                }
                if (debug)
                {
                    std::cout << "target_gimbal_speed.pitch: " << target_gimbal_speed.pitch << std::endl;
                    std::cout << "target_gimbal_speed.yaw: " << target_gimbal_speed.yaw << std::endl;
                }
                mpc_vel_pub_->publish(target_gimbal_speed);
            }

            if (debug)
            {
                std::cout << "target_gimbal_pose.pitch: " << target_gimbal_pose.pitch << std::endl;
                std::cout << "target_gimbal_pose.yaw: " << target_gimbal_pose.yaw << std::endl;
            }

            if (debug)
            {
                geometry_msgs::msg::Pose armor_pose_abs = target_armor_pose;
                geometry_msgs::msg::TransformStamped t_debug;
                if (getTransform(base_link_, shooter_link_, status_ptr_->header.stamp, t_debug)) // 获取绝对系到电机系的坐标变化
                {
                    tf2::doTransform(armor_pose_abs, armor_pose_abs, t_debug); // 将装甲板位姿从绝对系转到电机系
                }
                base_interfaces::msg::DebugOfKinematic solverDebug;
                solverDebug.armor_pose = armor_pose_abs;
                solverDebug.armors = status_ptr_->armors;
                solverDebug.id = status_ptr_->id;
                debug_pub->publish(solverDebug);
            }

            base_interfaces::msg::Shooter shoot_msg;
            shoot_msg.bulletnum = 0;
            if (this->enable_shoot && shootable(target_armor_pose, target_gimbal_pose)) // 判断是否被击打
            {
                builtin_interfaces::msg::Time t = this->now();
                double time = t.sec + t.nanosec * 1e-9;
                if (time - last_shooter_time > shoot_interval)
                {
                    last_shooter_time = time;
                    std::cout << "enable shoot!" << std::endl;
                    if (shoot_interval > 0)
                        shoot_msg.bulletnum = 1;
                    else
                        shoot_msg.bulletnum = 1;
                }
                if (status_ptr_->armors.size() > 0)
                {
                    white_frame_ = 0;
                }
                else
                {
                    white_frame_++;
                }
                if (white_frame_ > all_white_tolerance_stop_shoot_)
                    shoot_msg.bulletnum = 0;
            }
            else
            {
                std::cout << "not shoot!" << std::endl;
            }
            if (shoot_msg.bulletnum == 1)
                shoot_pub_->publish(shoot_msg);
        }
        else
        {
            std::cout << "no tracked armor" << std::endl;
        }
        mtx.unlock();
        return;
    }

    bool PoseSolver::getTransform(std::string tar_frame, std::string src_frame, builtin_interfaces::msg::Time stamp, geometry_msgs::msg::TransformStamped &transform)
    {
        try
        {
            transform = tf_buffer_->lookupTransform(
                tar_frame, src_frame,
                stamp,
                rclcpp::Duration::from_nanoseconds(50000));
        }
        catch (const tf2::TransformException &ex)
        {
            RCLCPP_INFO(
                this->get_logger(), "Could not transform %s to %s: %s",
                tar_frame.c_str(), src_frame.c_str(), ex.what());
            return false;
        }
        return true;
    }

    std::vector<base_interfaces::msg::Armor> PoseSolver::getArmors(double predict_time, int number)
    {
        std::vector<base_interfaces::msg::Armor> armors(number);
        if (number == 4) // STANDARD模型
        {
            for (size_t i = 0; i < number; i++)
            {
                // 装甲板yaw角，绝对系
                double angle = _std_radian(status_ptr_->phase + status_ptr_->palstance * predict_time + i * PI / 2);
                cv::Point2d point = cv::Point2d(status_ptr_->center.x, status_ptr_->center.y) +
                                    cv::Point2d(status_ptr_->velocity.x, status_ptr_->velocity.y) * predict_time +
                                    cv::Point2d(status_ptr_->radius[i % 2] * cos(angle), status_ptr_->radius[i % 2] * sin(angle));
                armors[i].armor_pose.position.x = point.x;
                armors[i].armor_pose.position.y = point.y;
                armors[i].armor_pose.position.z = status_ptr_->height[i % 2];
                // 将yaw角转四元数
                tf2::Quaternion q;
                q.setRPY(0, 0, angle);
                armors[i].armor_pose.orientation.x = q.x();
                armors[i].armor_pose.orientation.y = q.y();
                armors[i].armor_pose.orientation.z = q.z();
                armors[i].armor_pose.orientation.w = q.w();
                std::cout << "center: " << status_ptr_->center.x << std::endl;
                std::cout << "center: " << status_ptr_->center.y << std::endl;
            }
        }
        else if (number == 2) //  BALANCE模型
        {
            for (int i = 0; i < number; i++)
            {
                // 装甲板yaw角，绝对系
                double angle = _std_radian(status_ptr_->phase + status_ptr_->palstance * predict_time + i * PI);
                cv::Point2d point = cv::Point2d(status_ptr_->center.x, status_ptr_->center.y) +
                                    cv::Point2d(status_ptr_->velocity.x * cos(status_ptr_->phase) * predict_time,
                                                status_ptr_->velocity.x * sin(status_ptr_->phase) * predict_time) +
                                    cv::Point2d(status_ptr_->radius[0] * cos(angle), status_ptr_->radius[0] * sin(angle));
                armors[i].armor_pose.position.x = point.x;
                armors[i].armor_pose.position.y = point.y;
                armors[i].armor_pose.position.z = status_ptr_->height[0];
                // 将yaw角转四元数
                tf2::Quaternion q;
                q.setRPY(0, 0, angle);
                armors[i].armor_pose.orientation.x = q.x();
                armors[i].armor_pose.orientation.y = q.y();
                armors[i].armor_pose.orientation.z = q.z();
                armors[i].armor_pose.orientation.w = q.w();
            }
        }
        else if (number == 3) // OUTPOST模型
        {
            for (int i = 0; i < number; i++)
            {
                // 装甲板yaw角，绝对系
                double angle = _std_radian(status_ptr_->phase + status_ptr_->palstance * predict_time + 2 * i * PI / 3.0);
                cv::Point2d point = cv::Point2d(status_ptr_->center.x, status_ptr_->center.y) +
                                    cv::Point2d(status_ptr_->radius[0] * cos(angle), status_ptr_->radius[0] * sin(angle));
                armors[i].armor_pose.position.x = point.x;
                armors[i].armor_pose.position.y = point.y;
                armors[i].armor_pose.position.z = status_ptr_->height[i];
                // 将yaw角转四元数
                tf2::Quaternion q;
                q.setRPY(0, 0, angle);
                armors[i].armor_pose.orientation.x = q.x();
                armors[i].armor_pose.orientation.y = q.y();
                armors[i].armor_pose.orientation.z = q.z();
                armors[i].armor_pose.orientation.w = q.w();
            }
        }
        return armors;
    }

    bool PoseSolver::getClosestArmorPose(double predict_time, geometry_msgs::msg::Pose &armor_pose_out)
    {
        // 获取装甲板数量
        int number = 0;
        if (model_armors.count(status_ptr_->model_type))
        {
            number = model_armors[status_ptr_->model_type];
        }
        else
        {
            std::cout << "error model type: " << status_ptr_->model_type << std::endl;
            return false;
        }

        // 计算预测后装甲板的位姿
        if (debug)
        {
            this->get_parameter<float>("Aimer.switch_threshold", m_switch_threshold);
        }
        double switch_advanced_time = 0.2;
        if (status_ptr_->palstance != 0)
        {
            switch_advanced_time = std::min(0.2, D2R(m_switch_threshold) / abs(status_ptr_->palstance));
        }
        std::vector<base_interfaces::msg::Armor> armors = getArmors(predict_time + switch_advanced_time, number);
        geometry_msgs::msg::TransformStamped t;
        if (!getTransform(shooter_link_, base_link_, status_ptr_->header.stamp, t)) // 获取绝对系到电机系坐标变化
        {
            return false;
        }
        for (auto &armor : armors)
        {
            tf2::doTransform(armor.armor_pose, armor.armor_pose, t); // 将装甲板位姿从绝对系转到电机系
            tf2::Quaternion tf_q;
            tf2::fromMsg(armor.armor_pose.orientation, tf_q); // 将geometry_msgs::msg::Quaternion转成tf2::Quaternion消息
            double roll, pitch, yaw;
            tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); // 获取电机系下装甲板yaw角
            armor.yaw_angle = _std_radian(yaw);
        }

        // 计算预测后的机器人位置
        geometry_msgs::msg::Pose center; // OUTPOST模型
        center.position.x = status_ptr_->center.x;
        center.position.y = status_ptr_->center.y;
        if (status_ptr_->model_type == 0) // STANDAR模型
        {
            center.position.x += status_ptr_->velocity.x * predict_time;
            center.position.y += status_ptr_->velocity.y * predict_time;
        }
        else if (status_ptr_->model_type == 1) // BALANCE模型
        {
            center.position.x += status_ptr_->velocity.x * cos(status_ptr_->phase) * predict_time;
            center.position.y += status_ptr_->velocity.x * sin(status_ptr_->phase) * predict_time;
        }

        tf2::doTransform(center, center, t); // 将机器人坐标中心从绝对系转到电机系

        // 选择要击打的装甲板
        int index = 0;
        for (int i = 1; i < number; i++)
        {
            if (abs(_std_radian(PI + armors[i].yaw_angle - atan2(center.position.y, center.position.x))) + D2R(m_switch_threshold) <
                abs(_std_radian(PI + armors[index].yaw_angle - atan2(center.position.y, center.position.x))))
            {
                index = i;
            }
        }
        armor_pose_out = armors[index].armor_pose;
        if (debug)
        {
            if (predict_time != 0)
            {
                base_interfaces::msg::Armors armors_msg;
                for (int i = 0; i < armors.size(); i++)
                {
                    armors[i].header.frame_id = shooter_link_;
                    armors[i].header.stamp = status_ptr_->header.stamp;
                    armors_msg.armors.push_back(armors[i]);
                    armors_msg.num++;
                }
                base_interfaces::msg::Armor armor;
                armor.header.frame_id = shooter_link_;
                armor.header.stamp = status_ptr_->header.stamp;
                armor.armor_pose = center;
                armor.id = index;
                armors_msg.armors.push_back(armor);
                armors_pub->publish(armors_msg);
            }
        }
        return true;
    }

    bool PoseSolver::getFacingArmorPose(double predict_time, geometry_msgs::msg::Pose &armor_pose_out)
    {
        // 获取装甲板数量
        int number = 0;
        if (model_armors.count(status_ptr_->model_type))
        {
            number = model_armors[status_ptr_->model_type];
        }
        else
        {
            std::cout << "error model type: " << status_ptr_->model_type << std::endl;
            return false;
        }

        // 计算预测后装甲板的位姿
        std::vector<base_interfaces::msg::Armor> armors = getArmors(predict_time, number);
        for (auto &armor : armors)
        {
            geometry_msgs::msg::TransformStamped t;
            if (getTransform(shooter_link_, base_link_, status_ptr_->header.stamp, t)) // 获取绝对系到电机系坐标变化
            {
                tf2::doTransform(armor.armor_pose, armor.armor_pose, t); // 将装甲板位姿从绝对系转到电机系
            }
            else
            {
                return false;
            }
            tf2::Quaternion tf_q;
            tf2::fromMsg(armor.armor_pose.orientation, tf_q); // 将geometry_msgs::msg::Quaternion转成tf2::Quaternion消息
            double roll, pitch, yaw;
            tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw); // 获取电机系下装甲板yaw角
            armor.yaw_angle = _std_radian(yaw);
        }

        // 计算预测后的机器人位置
        geometry_msgs::msg::Pose center; // OUTPOST模型
        center.position.x = status_ptr_->center.x;
        center.position.y = status_ptr_->center.y;
        if (status_ptr_->model_type == 0) // STANDAR模型
        {
            center.position.x += status_ptr_->velocity.x * predict_time;
            center.position.y += status_ptr_->velocity.y * predict_time;
        }
        else if (status_ptr_->model_type == 1) // BALANCE模型
        {
            center.position.x += status_ptr_->velocity.x * cos(status_ptr_->phase) * predict_time;
            center.position.y += status_ptr_->velocity.x * sin(status_ptr_->phase) * predict_time;
        }
        geometry_msgs::msg::TransformStamped t;
        if (getTransform(shooter_link_, base_link_, status_ptr_->header.stamp, t)) // 获取绝对系到电机系坐标变化
        {
            tf2::doTransform(center, center, t); // 将机器人坐标中心从绝对系转到电机系
        }
        else
        {
            return false;
        }

        // 选择要击打的装甲板
        int index = 0;
        for (int i = 1; i < number; i++)
        {
            if (abs(_std_radian(PI + armors[i].yaw_angle - atan2(center.position.y, center.position.x))) <
                abs(_std_radian(PI + armors[index].yaw_angle - atan2(center.position.y, center.position.x))))
            {
                index = i;
            }
        }

        double angle = _std_radian(atan2(center.position.y, center.position.x) - PI);
        // 将yaw角转四元数
        tf2::Quaternion q;
        q.setRPY(0, 0, angle);
        armor_pose_out.orientation.x = q.x();
        armor_pose_out.orientation.y = q.y();
        armor_pose_out.orientation.z = q.z();
        armor_pose_out.orientation.w = q.w();
        if (status_ptr_->model_type == 0) // STANDARD模型
        {
            // 如果最近的装甲板处于远离状态，则击打下一装甲板
            if (abs(_std_radian(PI + armors[index].yaw_angle + (status_ptr_->palstance / abs(status_ptr_->palstance)) * D2R(-50) - atan2(center.position.y, center.position.x))) <
                abs(_std_radian(PI + armors[index].yaw_angle - atan2(center.position.y, center.position.x))))
            {
                index = (index + 1) % number;
            }
            armor_pose_out.position.x = center.position.x + status_ptr_->radius[index % 2] * cos(angle);
            armor_pose_out.position.y = center.position.y + status_ptr_->radius[index % 2] * sin(angle);
            armor_pose_out.position.z = status_ptr_->height[index % 2];
        }
        else // BALANCE模型 && OUTPOST模型
        {
            armor_pose_out.position.x = center.position.x + status_ptr_->radius[0] * cos(angle);
            armor_pose_out.position.z = center.position.y + status_ptr_->radius[0] * sin(angle);
            armor_pose_out.position.z = status_ptr_->height[index];
        }
        return true;
    }

    bool PoseSolver::getAngle(geometry_msgs::msg::Pose target_armor_pose, float bullet_speed, base_interfaces::msg::GimbalPose &gimbal_pose)
    {
        gimbal_pose.pitch = -std::atan(target_armor_pose.position.z / std::sqrt(target_armor_pose.position.x * target_armor_pose.position.x + target_armor_pose.position.y * target_armor_pose.position.y));
        gimbal_pose.yaw = std::atan2(target_armor_pose.position.y, target_armor_pose.position.x);
        std::cout << "111111armor" << target_armor_pose.position.x << std::endl;
        std::cout << "111111armor" << target_armor_pose.position.y << std::endl;
        std::cout << "111111armor" << target_armor_pose.position.z << std::endl;

        if (debug)
        {
            this->get_parameter<float>("Gimbal.pitch_off", pitch_off_);
            this->get_parameter<float>("Gimbal.yaw_off", yaw_off_);
        }
        gimbal_pose.pitch += pitch_off_;
        gimbal_pose.yaw += yaw_off_;

        if (debug)
        {
            this->get_parameter<bool>("Gimbal.fix_on", fix_on_);
        }
        if (fix_on_)
        {
            double dis = sqrt(target_armor_pose.position.x * target_armor_pose.position.x + target_armor_pose.position.y * target_armor_pose.position.y + target_armor_pose.position.z * target_armor_pose.position.z);
            double angle_fix = 0;
            if (abs(9.8 * dis * pow(cos(gimbal_pose.pitch), 2)) / pow(bullet_speed, 2) - sin(gimbal_pose.pitch) <= 1)
            {
                angle_fix = 0.5 * (asin((9.8 * dis * pow(cos(gimbal_pose.pitch), 2)) / pow(bullet_speed, 2) - sin(gimbal_pose.pitch)) + gimbal_pose.pitch);
            }
            else
            {
                std::cout << _warning("[AngleSolver] Wrong distance in angle_fix") << std::endl;
                return false;
            }
            gimbal_pose.pitch -= angle_fix;
        }
        return true;
    }

    bool PoseSolver::shootable(geometry_msgs::msg::Pose target_armor_pose, base_interfaces::msg::GimbalPose target_gimbal_pose)
    {
        /**
         * 判断自动击发，条件如下：
         * 1. 目标旋转速度小于一定阈值，或目标装甲板相对偏角不超过一定范围
         * 2. EKF先验稳定
         * 3. 没有长时间丢识别
         * 4. 当前云台跟随稳定，即当前位姿和目标位姿相近
         */
        if (status_ptr_->header.frame_id != shooter_link_)
        {
            std::cout << "[shootable]: false 0! shooter_link = " << shooter_link_ << " , status_link= " << status_ptr_->header.frame_id << std::endl;
            return false;
        }
        geometry_msgs::msg::Pose vector_c_pose;
        vector_c_pose.position.x = status_ptr_->center.x;
        vector_c_pose.position.y = status_ptr_->center.y;
        geometry_msgs::msg::TransformStamped _t;
        if (getTransform(shooter_link_, base_link_, status_ptr_->header.stamp, _t)) // 获取绝对系到电机系坐标变化
        {
            tf2::doTransform(vector_c_pose, vector_c_pose, _t); // 将机器人坐标中心从绝对系转到电机系
        }
        else
        {
            return false;
        }
        cv::Point2d vector_c = cv::Point2d(vector_c_pose.position.x, vector_c_pose.position.y);
        cv::Point2d vector_a = cv::Point2d(vector_c_pose.position.x - target_armor_pose.position.x, vector_c_pose.position.y - target_armor_pose.position.y);
        double abs_angle = R2D(acos((vector_c.x * vector_a.x + vector_c.y * vector_a.y) / (getDistance(vector_c) * getDistance(vector_a))));
        if (debug)
        {
            this->get_parameter<float>("Aimer.force_aim_palstance_threshold", m_force_aim_palstance_threshold);
            this->get_parameter<float>("Aimer.aim_angle_tolerance", m_aim_angle_tolerance);
        }
        if (abs(status_ptr_->palstance) < m_force_aim_palstance_threshold || abs_angle < m_aim_angle_tolerance)
        {
            if (debug)
            {
                this->get_parameter<float>("Aimer.CenterTrack.aim_center_angle_tolerance", m_aim_center_angle_tolerance);
            }
            if (m_enable_center_track && !(abs_angle < m_aim_center_angle_tolerance))
            {
                std::cout << "[shootable]: false 1! abs_angle = " << abs_angle << " < m_aim_center_angle_tolerance" << std::endl;
                return false;
            }
        }
        else
        {
            std::cout << "[shootable]: false 2! abs(status_ptr_->palstance): " << abs(status_ptr_->palstance) << " , abs_angle: " << abs_angle << std::endl;
            return false;
        }
        wmj::GimbalPose cur_pose;
        if (!getCurPose(cur_pose))
        {
            std::cout << "[shootable]: false 3!" << std::endl;
            return false;
        }
        double pitch, yaw;
        // geometry_msgs::msg::TransformStamped t;
        // // 获取云台yaw角
        // if(!getTransform(shooter_link_, shooter_yaw_link, this->now(), t))
        // {
        //     std::cout << "[shootable]: false 3!" << std::endl;
        //     return false;
        // }
        // tf2::Quaternion tf_q;
        // tf2::fromMsg(t.transform.rotation, tf_q);
        // tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw_);
        // // 获取云台pitch角
        // if(!getTransform(shooter_yaw_link, shooter_pitch_link, this->now(), t))
        // {
        //     std::cout << "[shootable]: false 3!" << std::endl;
        //     return false;
        // }
        // tf2::fromMsg(t.transform.rotation, tf_q);
        // tf2::Matrix3x3(tf_q).getRPY(roll, pitch_, yaw);

        yaw = cur_pose.yaw;
        // yaw = _std_radian(yaw);
        pitch = cur_pose.pitch;
        if (debug)
        {
            std::cout << "[shootable] current_pitch: " << pitch << std::endl;
            std::cout << "[shootable] current_yaw: " << yaw << std::endl;
            this->get_parameter<float>("Aimer.aim_pose_tolerance", m_aim_pose_tolerance);
        }
        double aim_pose_tolerance_pitch, aim_pose_tolerance_yaw;
        double distance = getDistance(cv::Point3d(target_armor_pose.position.x, target_armor_pose.position.y, target_armor_pose.position.z));
        if (status_ptr_->id == 1 || status_ptr_->model_type == 1)
        {
            aim_pose_tolerance_pitch = atan(0.130 / distance) * m_aim_pose_tolerance;
            aim_pose_tolerance_yaw = atan(0.260 / distance) * m_aim_pose_tolerance;
        }
        else
        {
            aim_pose_tolerance_pitch = atan(0.130 / distance) * m_aim_pose_tolerance;
            aim_pose_tolerance_yaw = atan(0.135 / distance) * m_aim_pose_tolerance;
        }
        std::cout << "111aim_pose_tolerance_pitch: " << aim_pose_tolerance_pitch << std::endl;
        std::cout << "111aim_pose_tolerance_yaw: " << aim_pose_tolerance_yaw << std::endl;
        if (abs(target_gimbal_pose.pitch - pitch) < aim_pose_tolerance_pitch && abs(target_gimbal_pose.yaw - yaw) < aim_pose_tolerance_yaw)
        {
            return true;
        }
        std::cout << "[shootable] target_yaw" << target_gimbal_pose.yaw - yaw << std::endl;

        std::cout << "[shootable]: false 4! "
                  << "std::hypot(target_gimbal_pose.pitch - pitch, target_ gimbal_pose.yaw - yaw): "
                  << _red(std::to_string(std::hypot(target_gimbal_pose.pitch - pitch, target_gimbal_pose.yaw - yaw)))
                  << std::endl;
        return false;
    }

    void PoseSolver::top_cb_(base_interfaces::msg::KinematicStatus::SharedPtr status_msg)
    {
        // 加锁防止读写错误
        mtx.lock();
        std::cout << "[PoseSolver]model update" << std::endl;
        status_ptr_ = status_msg; // 保存当前模型，根据当前模型进行控制
        mtx.unlock();
    }

    void PoseSolver::bt_cb(const base_interfaces::msg::BtAimer &msg)
    {
        this->enable_shoot = msg.aimer_status;
        this->keep_scan = msg.scan_is_open;
    }

    /**
     * 在传统代码中，mpc 是以库的形式直接调用返回速度环
     * 由于发弹窗口的判断需要位置环得出的目标云台角度，因此代码建立在位置环的基础上
     */

    bool PoseSolver::getGimbalSpeed(base_interfaces::msg::GimbalPose &target_gimbal_speed)
    {

        // 根据运动模型预测控制目标
        Eigen::MatrixXd predict_ctrl_target;
        if (!getPredictControlTarget(predict_ctrl_target))
        {
            return false;
        }
        predict_ctrl_target.conservativeResize(predict_ctrl_target.rows() * predict_ctrl_target.cols(), 1);
        // std::cout << "[MPC] Predict_Ctrl_Target: " << std::endl << predict_ctrl_target << std::endl;

        // 求解最优控制
        Eigen::Matrix<double, 2, 1> cur_state;
        wmj::GimbalPose cur_pose;
        if (!getCurPose(cur_pose))
        {
            return false;
        }
        cur_state << cur_pose.pitch, cur_pose.yaw;
        // std::cout << "curpose" << cur_pose.yaw << " , "<< cur_pose.pitch << std::endl;
        Eigen::MatrixXd U = solveQuadProg(m_H, m_E * cur_state - m_Ed * predict_ctrl_target);

        target_gimbal_speed.pitch = U(0, 0);
        target_gimbal_speed.yaw = U(1, 0);
        return true;
    }
    bool PoseSolver::getPredictControlTarget(Eigen::MatrixXd &predict_ctrl_target)
    {
        // 控制目标
        predict_ctrl_target.resize(_D_STATE_, m_step + 1);
        // std::cout << "[MPC]: " << predict_ctrl_target.cols() << predict_ctrl_target.rows() << predict_ctrl_target << std::endl;
        predict_ctrl_target.setZero();

        // 判断是否锁中心
        if (m_track_center &&
            ((abs(status_ptr_->palstance) - m_switch_trackmode_threshold) > m_aim_center_palstance_threshold))
        {
            m_enable_center_track = true;
        }
        else
        {
            m_enable_center_track = false;
        }

        for (int i = 0; i <= m_step; i++)
        {
            base_interfaces::msg::GimbalPose pose;
            if (!getTargetPose(pose, m_dt * i))
            {
                return false;
            }
            predict_ctrl_target(0, i) = pose.pitch;
            predict_ctrl_target(1, i) = pose.yaw;
        }
        // 若状态量包含速度
        if (_D_STATE_ == 4)
        {
            for (int i = 0; i < m_step; i++)
            {
                predict_ctrl_target(2, i) = (predict_ctrl_target(0, i + 1) - predict_ctrl_target(0, i)) / m_dt;
                predict_ctrl_target(3, i) = (predict_ctrl_target(1, i + 1) - predict_ctrl_target(1, i)) / m_dt;
            }
            base_interfaces::msg::GimbalPose pose;
            if (!getTargetPose(pose, m_dt * (m_step + 1)))
            {
                return false;
            }
            predict_ctrl_target(2, m_step) = (pose.pitch - predict_ctrl_target(0, m_step)) / m_dt;
            predict_ctrl_target(3, m_step) = (pose.yaw - predict_ctrl_target(1, m_step)) / m_dt;
        }
        if (debug)
            std::cout << "[MPC] predict_ctrl_target: " << std::endl
                      << predict_ctrl_target << std::endl;

        return true;
    }

    /**
     * 根据模型获得未来指定时刻的云台位姿
     * getclosearmor 会选择一个装甲板，然后往后推好几步，这个是从模型中拿出来的，把这个不带预测时间的模型求出装甲板然后和当前云台位姿进行比较，之后就能得到 shootable 的判断
     */

    bool PoseSolver::getTargetPose(base_interfaces::msg::GimbalPose &pose, double time_off)
    {
        // getCloseArmor 和 getAngle 放在一起组成新功能
        // 从模型得出要击打的装甲板，然后获得目标云台位姿

        geometry_msgs::msg::Pose target_armor_pose;

        double start_time, end_time, model_time_off;
        start_time = status_ptr_->header.stamp.sec + status_ptr_->header.stamp.nanosec * 1e-9;
        builtin_interfaces::msg::Time t = this->now();
        end_time = t.sec + t.nanosec * 1e-9;
        model_time_off = end_time - start_time;

        if (!getClosestArmorPose(model_time_off, target_armor_pose))
        {
            // mtx.unlock();
            return false;
        }
        double predict_time = getDistance(cv::Point3d(target_armor_pose.position.x, target_armor_pose.position.y, target_armor_pose.position.z)) / m_bullet_speed + model_time_off + m_time_off;

        if (!getClosestArmorPose(time_off + predict_time, target_armor_pose))
        {
            // mtx.unlock();
            return false;
        }

        if (!getAngle(target_armor_pose, m_bullet_speed, pose)) // 获取目标云台位姿，到枪口的距离算补偿
        {
            // mtx.unlock();
            return false;
        }

        return true;
    }

    void PoseSolver::setMpcMatrix()
    {
        // 初始化系统参数
        m_A.resize(_D_STATE_, _D_STATE_);
        m_B.resize(_D_STATE_, _D_CTRL_);
        m_Q.resize(_D_STATE_, _D_STATE_);
        m_R.resize(_D_CTRL_, _D_CTRL_);
        m_F.resize(_D_STATE_, _D_STATE_);

        m_A << 1, 0,
            0, 1;
        m_B << m_dt, 0,
            0, m_dt;

        m_Q = m_process_error_weight.asDiagonal();
        m_R = m_process_control_weight.asDiagonal();
        m_F = m_target_error_weight.asDiagonal();

        // if (m_debug)
        // {
        //     std::cout << "[MPC] A: " << std::endl << m_A << std::endl;
        //     std::cout << "[MPC] B: " << std::endl << m_B << std::endl;
        //     std::cout << "[MPC] Q: " << std::endl << m_Q << std::endl;
        //     std::cout << "[MPC] R: " << std::endl << m_R << std::endl;
        //     std::cout << "[MPC] F: " << std::endl << m_F << std::endl;
        // }

        // 初始化中间量
        Eigen::MatrixXd M;
        Eigen::MatrixXd C;
        M.resize((m_step + 1) * _D_STATE_, _D_STATE_);
        C.resize((m_step + 1) * _D_STATE_, m_step * _D_STATE_);
        M.setZero();
        C.setZero();

        Eigen::MatrixXd tmp = Eigen::Matrix<double, _D_STATE_, _D_STATE_>::Identity();
        M.block<_D_STATE_, _D_STATE_>(0, 0) = Eigen::Matrix<double, _D_STATE_, _D_STATE_>::Identity();
        for (int i = 1; i <= m_step; i++)
        {
            C.block<_D_STATE_, _D_CTRL_>(i * _D_STATE_, 0) = tmp * m_B;
            for (int j = 1; j < m_step; j++)
            {
                C.block<_D_STATE_, _D_CTRL_>(i * _D_STATE_, j * _D_CTRL_) = C.block<_D_STATE_, _D_CTRL_>((i - 1) * _D_STATE_, (j - 1) * _D_CTRL_);
            }
            tmp *= m_A;
            M.block<_D_STATE_, _D_STATE_>(i * _D_STATE_, 0) = tmp;
        }

        // if (m_debug)
        // {
        //     std::cout << "[MPC] M: " << std::endl << M << std::endl;
        //     std::cout << "[MPC] C: " << std::endl << C << std::endl;
        // }

        Eigen::MatrixXd Q_;
        Eigen::MatrixXd R_;
        Q_.resize((m_step + 1) * _D_STATE_, (m_step + 1) * _D_STATE_);
        R_.resize(m_step * _D_CTRL_, m_step * _D_CTRL_);
        Q_.setZero();
        R_.setZero();

        for (int i = 0; i < m_step; i++)
        {
            Q_.block<_D_STATE_, _D_STATE_>(i * _D_STATE_, i * _D_STATE_) = m_Q;
            R_.block<_D_CTRL_, _D_CTRL_>(i * _D_CTRL_, i * _D_CTRL_) = m_R;
        }
        Q_.block<_D_STATE_, _D_STATE_>(m_step * _D_STATE_, m_step * _D_STATE_) = m_F;

        // if (m_debug)
        // {
        //     std::cout << "[MPC] Q_: " << std::endl << Q_ << std::endl;
        //     std::cout << "[MPC] R_: " << std::endl << R_ << std::endl;
        // }

        // 计算损失函数系数
        m_H = C.transpose() * Q_ * C + R_;
        m_E = C.transpose() * Q_ * M;
        m_Ed = C.transpose() * Q_;

        // if (m_debug)
        // {
        //     std::cout << "[MPC] H: " << std::endl << m_H << std::endl;
        //     std::cout << "[MPC] E: " << std::endl << m_E << std::endl;
        //     std::cout << "[MPC] Ed: " << std::endl << m_Ed << std::endl;
        // }
    }
    // //namespace wmj

    Eigen::MatrixXd PoseSolver::solveQuadProg(Eigen::MatrixXd H, Eigen::MatrixXd f)
    {
        // 迭代中间量，在循环中可以看作是上一次迭代的结果
        Eigen::MatrixXd x0;
        Eigen::MatrixXd r0;
        Eigen::MatrixXd p0;
        x0 = f;
        x0.setZero();
        r0 = H * x0 + f;
        p0 = r0;

        // 迭代更新量
        Eigen::MatrixXd x;
        Eigen::MatrixXd r;
        Eigen::MatrixXd p;
        Eigen::MatrixXd alpha;
        Eigen::MatrixXd beta;

        for (int i = 0; i < 1000; i++)
        {
            alpha = (r0.transpose() * r0) * (p0.transpose() * H * p0).inverse();
            x = x0 - alpha(0, 0) * p0;
            r = r0 - alpha(0, 0) * H * p0;
            beta = (r.transpose() * r) * (r0.transpose() * r0).inverse();
            p = r + beta(0, 0) * p0;

            if ((x - x0).norm() < 1e-8)
            {
                if (0)
                {
                    std::cout << "[MPC] quadratic_result: " << std::endl
                              << x.transpose() << std::endl;
                    std::cout << x.cols() << " " << x.rows() << std::endl;
                }
                return x;
            }

            x0 = x;
            r0 = r;
            p0 = p;
        }

        std::cout << _warning("[MPC] Failed to solve the quadratic programming in limited times") << std::endl;
        return x;
    }
    bool PoseSolver::getCurPose(wmj::GimbalPose &pose)
    {
        double roll, pitch, yaw, pitch_, yaw_;
        geometry_msgs::msg::TransformStamped t;
        auto now = this->now();
        // 获取云台yaw角
        try
        {
            t = tf_buffer_->lookupTransform(
                shooter_link_, shooter_yaw_link,
                tf2::TimePoint());
        }
        catch (const tf2::TransformException &ex)
        {
            std::cout << "false 3!" << std::endl;

            return false;
        }
        // if (!getTransform(shooter_link_, shooter_yaw_link, now, t))
        // {
        //     std::cout << "false 3!" << std::endl;
        //     return false;
        // }
        tf2::Quaternion tf_q;
        tf2::fromMsg(t.transform.rotation, tf_q);
        tf2::Matrix3x3(tf_q).getRPY(roll, pitch, yaw_);
        // 获取云台pitch角
        try
        {
            t = tf_buffer_->lookupTransform(
                shooter_yaw_link, shooter_pitch_link,
                tf2::TimePoint());
        }
        catch (const tf2::TransformException &ex)
        {
            std::cout << "false 3!" << std::endl;

            return false;
        }

        // if (!getTransform(shooter_yaw_link, shooter_pitch_link, now, t))
        // {
        //     std::cout << "false 3!" << std::endl;
        //     return false;
        // }
        tf2::fromMsg(t.transform.rotation, tf_q);
        tf2::Matrix3x3(tf_q).getRPY(roll, pitch_, yaw);
        pose.yaw = yaw_;
        pose.pitch = pitch_;
        return true;
    }

} // namespace wmj

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto poseSolver = std::make_shared<wmj::PoseSolver>("PoseSolver");
    rclcpp::spin(poseSolver);
    rclcpp::shutdown();
    return 0;
}
